import rclpy
from rclpy.node import Node
import serial
import time
import struct

class DogControlNode(Node):
    def __init__(self, uart_dev, baudrate):
        super().__init__('dog_control')
        self.get_logger().info("Successfully Login1.")
        
        # 初始化串口
        try:
            self.ser = serial.Serial(uart_dev, baudrate, timeout=1)
            self.get_logger().info(f"Serial port {uart_dev} opened with baudrate {baudrate}.")
        except Exception as e:
            self.get_logger().error(f"Failed to open serial port: {e}")
            raise

        # 创建一个定时器，每隔1秒发送一次数据
        self.timer = self.create_timer(1.0, self.timer_callback)

        # 初始化8个舵机的目标角度（0-180度）
        self.servo_angles = [90, 90, 90, 90, 90, 90, 90, 90]  # 初始化为90度

    def timer_callback(self):
        # 更新舵机角度（这里可以根据需要动态修改角度）
        self.update_servo_angles()

        # 发送舵机角度数据
        self.send_servo_angles()

    def update_servo_angles(self):
        """
        更新8个舵机的目标角度。
        这里可以根据需要动态修改舵机角度。
        """
        # 示例：让每个舵机的角度在0-180度之间循环增加
        for i in range(len(self.servo_angles)):
            self.servo_angles[i] = (self.servo_angles[i] + 10) % 180

    def send_servo_angles(self):
        """
        将8个舵机的目标角度打包并发送到下位机。
        """
        try:
            # 将舵机角度打包为字节流（每个角度占1个字节）
            data = struct.pack('8B', *self.servo_angles)
            
            # 发送数据
            self.ser.write(data)
            self.get_logger().info(f"Sent servo angles: {self.servo_angles}")
            
            # 读取下位机返回的数据（可选）
            received_data = self.ser.read(self.ser.in_waiting or 1)
            if received_data:
                self.get_logger().info(f"Received: {received_data}")
        except Exception as e:
            self.get_logger().error(f"Serial communication error: {e}")

    def __del__(self):
        # 关闭串口
        if hasattr(self, 'ser') and self.ser.is_open:
            self.ser.close()
            self.get_logger().info("Serial port closed.")

def main(args=None):
    rclpy.init(args=args)
    
    # 串口设备名称和波特率
    uart_dev = "/dev/ttyS1"  # 根据实际情况修改
    baudrate = 115200  # 根据实际情况修改

    # 创建节点实例
    node_dog_control = DogControlNode(uart_dev, baudrate)
    
    # 运行节点
    rclpy.spin(node_dog_control)
    
    # 关闭节点
    node_dog_control.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()